#include "delaunator.hpp"
#include <chrono>
#include <eigen3/Eigen/Dense>
#include <fstream>
#include <iostream>
#include <vector>

#include <algorithm>
#include <cmath>
#include <unordered_set>

namespace delaunator {

using Point = Eigen::Vector2d;
using Path = std::vector<Point>;

struct Node {
  Point p;

  Node(const Point &p) : p(p) {}

  std::vector<std::shared_ptr<Node>> neighbors;

  using Ptr = std::shared_ptr<Node>;
};

// 保存三角剖分结果到文件
void saveTriangles2file(const std::vector<std::size_t> &triangles,
                        const std::vector<double> &coords,
                        const std::string &filename) {
  std::ofstream file(filename);
  if (!file.is_open()) {
    std::cerr << "Failed to open file: " << filename << std::endl;
    return;
  }

  file << "POINTS\n";
  for (std::size_t i = 0; i < coords.size(); i += 2) {
    file << coords[i] << " " << coords[i + 1] << "\n";
  }

  file << "TRIANGLES\n";
  for (std::size_t i = 0; i < triangles.size(); i += 3) {
    file << triangles[i] << " " << triangles[i + 1] << " " << triangles[i + 2]
         << "\n";
  }

  file.close();
  std::cout << "Delaunay triangulation saved to " << filename << std::endl;
}

void saveGraph2File(const delaunator::Node::Ptr &startNode,
                    const std::string &filename) {
  std::ofstream outFile(filename);
  if (!outFile) {
    std::cerr << "Unable to open file for writing!" << std::endl;
    return;
  }

  std::unordered_set<delaunator::Node::Ptr> visited;

  std::function<void(delaunator::Node::Ptr)> dfs =
      [&](delaunator::Node::Ptr node) {
        if (visited.find(node) != visited.end()) {
          return;
        }
        visited.insert(node);

        outFile << node->p.x() << " " << node->p.y() << "\n";

        for (const auto &neighbor : node->neighbors) {
          outFile << "--" << neighbor->p.x() << " " << neighbor->p.y() << "\n";
        }

        for (const auto &neighbor : node->neighbors) {
          dfs(neighbor);
        }
      };

  dfs(startNode);
  outFile.close();
  std::cout << "Graph saved to " << filename << std::endl;
}

void savePaths2File(const std::vector<Path> &paths,
                    const std::string &filename) {
  std::ofstream outFile(filename);
  if (!outFile) {
    std::cerr << "Unable to open file for writing!" << std::endl;
    return;
  }

  for (const auto &path : paths) {
    for (const auto &point : path) {
      outFile << point.x() << " " << point.y() << "\n";
    }
    outFile << "\n";
  }

  outFile.close();
  std::cout << "Paths saved to " << filename << std::endl;
}

class DelaunatorTopoPlanner {
public:
  DelaunatorTopoPlanner() {}

  void findPaths(const Point &start, const Point &end, const Path &points) {
    start_ = start;
    end_ = end;
    generateHexagon(points);

    // points_ = points;

    std::vector<double> coords(2 * points_.size());
    for (size_t i = 0; i < points_.size(); ++i) {
      coords[2 * i] = points_[i].x();
      coords[2 * i + 1] = points_[i].y();
    }
    try {
      auto start = std::chrono::high_resolution_clock::now();
      delaunator::Delaunator d(coords);
      auto end = std::chrono::high_resolution_clock::now();
      std::chrono::duration<double> elapsed_seconds = end - start;
      std::cout << "Delaunay triangulation took " << elapsed_seconds.count()
                << "s\n";

      std::vector<Node::Ptr> nodes =
          getGraph(d.getCoords(), d.getTriangles(), d.getHalfedges());

      findClosestNodes(nodes);

      if (startNode_ == nullptr || endNode_ == nullptr ||
          startNode_ == endNode_) {
        std::cerr << "Failed to find start and end nodes!" << std::endl;
        return;
      }

      std::vector<Path> paths;
      Path path;
      std::unordered_set<Node::Ptr> visited;
      dfs(startNode_, endNode_, path, paths, visited);
      sort(paths.begin(), paths.end(),
           [](const Path &a, const Path &b) { return a.size() < b.size(); });

      paths.resize(std::min(6, (int)paths.size()));

      // 将结果保存到文件
      saveTriangles2file(d.getTriangles(), coords, "triangulation.txt");
      saveGraph2File(nodes.front(), "graph.txt");
      savePaths2File(paths, "paths.txt");
    } catch (const std::exception &e) {
      std::cerr << "Error: " << e.what() << std::endl;
    }
  }

private:
  Path points_;
  Point start_;
  Point end_;
  Point dir_;
  Node::Ptr startNode_{nullptr};
  Node::Ptr endNode_{nullptr};
  double max_width_2_{3.0};
  double padding_{0.5};
  double max_backoff_cos_{0.0};

  void generateHexagon(const Path &points) {
    points_.clear();
    points_.reserve(points.size() + 6);
    dir_ = (end_ - start_).normalized();
    Point mid = (start_ + end_) / 2;
    Point normal = {dir_.y(), -dir_.x()};
    Eigen::Matrix2d R, R_inv;
    R << dir_, normal;
    R_inv = R.inverse();

    auto toLocal = [&](const Point &p) -> Point { return R_inv * (p - mid); };
    auto toGlobal = [&](const Point &p) -> Point { return R * p + mid; };

    Point s = toLocal(start_);
    Point e = toLocal(end_);
    points_.push_back(toGlobal(s - Point{padding_, 0}));
    points_.push_back(toGlobal(e + Point{padding_, 0}));

    double min_w, max_w;
    min_w = max_w = 0.0;

    for (const auto &p : points) {
      Point local_p = toLocal(p);
      if (std::abs(local_p.y()) > max_width_2_ || local_p.x() <= s.x() ||
          local_p.x() >= e.x()) {
        continue;
      }
      points_.push_back(p);
      if (local_p.y() < 0) {
        min_w = std::min(min_w, local_p.y());
      } else {
        max_w = std::max(max_w, local_p.y());
      }
    }

    min_w -= padding_;
    max_w += padding_;
    double x = (e.x() + padding_) / 3.0;
    points_.push_back(toGlobal(Point{x, min_w}));
    points_.push_back(toGlobal(Point{x, max_w}));
    points_.push_back(toGlobal(Point{-x, min_w}));
    points_.push_back(toGlobal(Point{-x, max_w}));
  }

  std::vector<Node::Ptr>
  getGraph(const std::vector<double> &coords,
           const std::vector<std::size_t> &triangles,
           const std::vector<std::size_t> &halfedges) const {

    std::vector<Node::Ptr> nodes;
    for (size_t i = 0; i < triangles.size(); i += 3) {
      Point p0{coords[2 * triangles[i]], coords[2 * triangles[i] + 1]};
      Point p1{coords[2 * triangles[i + 1]], coords[2 * triangles[i + 1] + 1]};
      Point p2{coords[2 * triangles[i + 2]], coords[2 * triangles[i + 2] + 1]};
      Point center = (p0 + p1 + p2) / 3.0;
      Node::Ptr node = std::make_shared<Node>(center);
      nodes.push_back(node);
    }

    auto linkNodes = [&](Node::Ptr a, Node::Ptr b) {
      a->neighbors.push_back(b);
      b->neighbors.push_back(a);
    };

    std::vector<std::size_t> _halfedges = halfedges;
    for (size_t i = 0; i < _halfedges.size(); ++i) {
      if (_halfedges[i] == INVALID_INDEX)
        continue;

      size_t j = _halfedges[i];
      _halfedges[j] = INVALID_INDEX; // mark as visited

      Point pi{coords[2 * triangles[i]], coords[2 * triangles[i] + 1]};
      Point pj{coords[2 * triangles[j]], coords[2 * triangles[j] + 1]};

      Node::Ptr node_cen = std::make_shared<Node>((pi + pj) / 2.0);
      linkNodes(nodes[i / 3], node_cen);
      linkNodes(nodes[j / 3], node_cen);
      nodes.push_back(node_cen);
    }

    return nodes;
  }

  void findClosestNodes(const std::vector<Node::Ptr> &nodes) {
    startNode_.reset();
    endNode_.reset();
    double minDistStart = std::numeric_limits<double>::max();
    double minDistEnd = std::numeric_limits<double>::max();

    for (const auto &node : nodes) {
      double distStart = (node->p - start_).norm();
      double distEnd = (node->p - end_).norm();

      if (distStart < minDistStart) {
        minDistStart = distStart;
        startNode_ = node;
      }

      if (distEnd < minDistEnd) {
        minDistEnd = distEnd;
        endNode_ = node;
      }
    }
  }

  void dfs(const Node::Ptr &current, const Node::Ptr &end, Path &path,
           std::vector<Path> &allPaths,
           std::unordered_set<Node::Ptr> &visited) {

    visited.insert(current);
    path.push_back(current->p);

    if (current == end) {
      allPaths.push_back(path);
    } else {
      for (const auto &neighbor : current->neighbors) {
        if (visited.find(neighbor) != visited.end())
          continue;

        /**
         * @note: 限制路径的方向
         *  注释掉是因为，delanator
         * 生成的三角形的质心或者边中点不一定就是最后的路径点，
         *  用下面的方法限制路径的方向会导致遗漏掉一些有效路径
         */
        // if (!isDirValid(current->p, neighbor->p)) continue;

        dfs(neighbor, end, path, allPaths, visited);
      }
    }

    // 回溯：撤销当前节点
    path.pop_back();
    visited.erase(current);
  }

  bool isDirValid(const Point &start, const Point &end) const {
    return (end - start).normalized().dot(dir_) > max_backoff_cos_;
  }
};

} // namespace delaunator

int main() {

  std::vector<Eigen::Vector2d> points;
  points = {{0, 0}, {1, 0}, {1, 1}, {0, 1}, {2, 0}};

  Eigen::Vector2d start = {-1, -1};
  Eigen::Vector2d end = {3, 2};

  delaunator::DelaunatorTopoPlanner planner;
  planner.findPaths(start, end, points);

  return 0;
}
